/*
 * Copyright (c) 2012, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Humanoid Robotics Lab      Georgia Institute of Technology
 * Director: Mike Stilman     http://www.golems.org
 */

#include "tabMultiPlanner.h"

#include <wx/wx.h>
#include <GUI/Viewer.h>
#include <GUI/GUI.h>
#include <GUI/GRIPSlider.h>
#include <GUI/GRIPFrame.h>
#include <Tabs/GRIPTab.h>
#include <robotics/Robot.h>

#include <iostream>
#include <algorithm>
#include <iterator>
#include <list>

#include <Tabs/AllTabs.h>
#include <GRIPApp.h>


/* Quick intro to adding tabs:
 * 1- Copy template cpp and header files and replace with new class name
 * 2- include classname.h in AllTabs.h, and use the ADD_TAB macro to create it
 */

// Control IDs (used for event handling - be sure to start with a non-conflicted id)

enum planTabEvents {
	button_SetStart = 50,
	button_SetGoal,
	button_showStart,
	button_showGoal,
	button_planSeparate,
	button_planHD,
	//button_resetPlanner,
	//button_empty1,
	//button_empty2,
	//button_Plan,
	//button_Stop,
	button_UpdateTime,
	button_UpdateGreedyRatio,
	//button_UpdateRobotId,
	button_ExportSequence,
	button_ShowPath,
	checkbox_beGreedy,
	checkbox_useConnect,
	checkbox_useSmooth,
	slider_Time,
};

// sizer for whole tab
wxBoxSizer* sizerFull2;

//Add a handler for any events that can be generated by the widgets you add here (sliders, radio, checkbox, etc)
BEGIN_EVENT_TABLE(MultiPlannerTab, wxPanel)
EVT_COMMAND (wxID_ANY, wxEVT_GRIP_SLIDER_CHANGE, MultiPlannerTab::OnSlider)
EVT_COMMAND (wxID_ANY, wxEVT_COMMAND_RADIOBOX_SELECTED, MultiPlannerTab::OnRadio)
EVT_COMMAND (wxID_ANY, wxEVT_COMMAND_BUTTON_CLICKED, MultiPlannerTab::OnButton)
EVT_COMMAND (wxID_ANY, wxEVT_COMMAND_CHECKBOX_CLICKED, MultiPlannerTab::OnCheckBox)
END_EVENT_TABLE()

// Class constructor for the tab: Each tab will be a subclass of RSTTab
IMPLEMENT_DYNAMIC_CLASS(MultiPlannerTab, GRIPTab)

/**
 * @function MultiPlannerTab
 * @brief Constructor
 */
MultiPlannerTab::MultiPlannerTab( wxWindow *parent, const wxWindowID id,
		              const wxPoint& pos, const wxSize& size, long style) :
	                      GRIPTab(parent, id, pos, size, style) {

    // Attributes for the HD RRT
    mStartConf.resize(0);
    mGoalConf.resize(0);
    mLinks.resize(0);

    // Initial values for parameters
	robotId = -1;
    mRrtStyle = 0;
    mGreedyMode = false;
    mConnectMode = false;
    mSmooth = false;
    mPlanner = NULL;

    sizerFull2 = new wxBoxSizer( wxHORIZONTAL );

    // ** Create left static box for configuring the planner **

    // Create StaticBox container for all items
    wxStaticBox* configureBox = new wxStaticBox(this, -1, wxT("Configure"));

    // Create sizer for this box with horizontal layout
    wxStaticBoxSizer* configureBoxSizer = new wxStaticBoxSizer(configureBox, wxHORIZONTAL);

    // Create a sizer for radio buttons in 1st column
    wxBoxSizer *col1Sizer = new wxBoxSizer(wxVERTICAL);
    wxBoxSizer *miniSizer = new wxBoxSizer(wxVERTICAL); // annoying hack to get checkboxes close together
    miniSizer->Add( new wxCheckBox(this, checkbox_beGreedy, _T("&goal bias (be greedy)")),
		    1, // vertical stretch evenly
		    wxALIGN_NOT,
		    0);
    miniSizer->Add( new wxCheckBox(this, checkbox_useConnect, _T("use &connect algorithm (be really greedy)")),
		    1, // vertical stretch evenly
		    wxALIGN_NOT,
		    0 );
    miniSizer->Add( new wxCheckBox(this, checkbox_useSmooth, _T("use &smoother (make it less ugly)")),
		    1, // vertical stretch evenly
		    wxALIGN_NOT,
		    0 );
    col1Sizer->Add(miniSizer,1,wxALIGN_NOT,0);

    // Create radio button for rrt_style
    static const wxString RRTStyles[] =
    {
        wxT("Single"),
	wxT("Bi-directional")
    };
    col1Sizer->Add( new wxRadioBox(this, wxID_ANY, wxT("RRT &style:"),
		    wxDefaultPosition, wxDefaultSize, WXSIZEOF(RRTStyles), RRTStyles, 1,
		    wxRA_SPECIFY_ROWS),
		    1, // stretch evenly with buttons and checkboxes
		    wxALIGN_NOT,
		    0 );
    // Add col1 to configureBoxSizer
    configureBoxSizer->Add( col1Sizer,
			    2, // 3/5 of configure box
			    wxALIGN_NOT,
			    0 ); //


    // Create StaticBox container for all items
    wxStaticBox* problemBox = new wxStaticBox(this, -1, wxT("Problem"));

    // Create sizer for this box with horizontal layout
    wxStaticBoxSizer* problemBoxSizer = new wxStaticBoxSizer(problemBox, wxHORIZONTAL);

    // Create sizer for start buttons in 2nd column
    wxBoxSizer *col2Sizer = new wxBoxSizer(wxVERTICAL);
    col2Sizer->Add( new wxButton(this, button_SetStart, wxT("Set &Start")),
		    0, // make horizontally unstretchable
		    wxALL, // make border all around (implicit top alignment)
		    1 ); // set border width to 1, so start buttons are close together
    col2Sizer->Add( new wxButton(this, button_showStart, wxT("Show S&tart")),
		    0, // make horizontally unstretchable
		    wxALL, // make border all around (implicit top alignment)
		    1 ); // set border width to 1, so start buttons are close together
    //col2Sizer->Add( new wxButton(this, button_empty1, wxT("Check collision")),
	//	    0, // make horizontally unstretchable
	//	    wxALL, // make border all around (implicit top alignment)
	//	    1 ); // set border width to 1, so start buttons are close together

    col2Sizer->Add( new wxButton(this, button_SetGoal, wxT("Set &Goal")),
		    0, // make horizontally unstretchable
		    wxALL, // make border all around (implicit top alignment)
		    1 ); // set border width to 1, so start buttons are close together
    col2Sizer->Add( new wxButton(this, button_showGoal, wxT("Show G&oal")),
		    0, // make horizontally unstretchable
		    wxALL, // make border all around (implicit top alignment)
		    1 ); // set border width to 1, so start buttons are close together


    // Add col2Sizer to the configuration box
    problemBoxSizer->Add( col2Sizer,
			    1, // takes half the space of the configure box
			    wxALIGN_NOT ); // no border and center horizontally


    // Add this box to parent sizer
    sizerFull2->Add( configureBoxSizer,
		    2, // 4-to-1 ratio with execute sizer, since it just has 3 buttons
		    wxEXPAND | wxALL,
		    6 );

    // Add this box to parent sizer
    sizerFull2->Add( problemBoxSizer,
		    1, // 4-to-1 ratio with execute sizer, since it just has 3 buttons
		    wxEXPAND | wxALL,
		    6 );




    // ** Create right static box for running the planner **
    wxStaticBox* executeBox = new wxStaticBox(this, -1, wxT("Planner"));

    // Create sizer for this box
    wxStaticBoxSizer* executeBoxSizer = new wxStaticBoxSizer(executeBox, wxVERTICAL);

    // Add buttons for "plan", "save movie", and "show path"
    executeBoxSizer->Add( new wxButton(this, button_planHD, wxT("&Plan HD")),
	 		  1, // stretch to fit horizontally
			  wxGROW ); // let it hog all the space in it's column

			   // Add buttons for "plan", "save movie", and "show path"
    executeBoxSizer->Add( new wxButton(this, button_planSeparate, wxT("&Plan Separate")),
	 		  1, // stretch to fit horizontally
			  wxGROW ); // let it hog all the space in it's column

    //executeBoxSizer->Add( new wxButton(this, button_Stop, wxT("&Stop")),
	//		  1, // stretch to fit horizontally
	//		  wxGROW );

    // ** Create right static box for running the planner **
    wxStaticBox* displayBox = new wxStaticBox(this, -1, wxT("Vizu"));
     // Create sizer for this box
    wxStaticBoxSizer* displayBoxSizer = new wxStaticBoxSizer(displayBox, wxVERTICAL);


    wxBoxSizer *timeSizer = new wxBoxSizer(wxHORIZONTAL);
    mTimeText = new wxTextCtrl(this,1008,wxT("5.0"),wxDefaultPosition,wxSize(40,20),wxTE_RIGHT);//,wxTE_PROCESS_ENTER | wxTE_RIGHT);
    timeSizer->Add( mTimeText,2,wxALL,1 );
    timeSizer->Add(new wxButton(this, button_UpdateTime, wxT("Set T(s)")),2,wxALL,1);
    displayBoxSizer->Add(timeSizer,1,wxALL,2);

    displayBoxSizer->Add( new wxButton(this, button_ShowPath, wxT("&Print")),
			  1, // stretch to fit horizontally
			  wxGROW );


    sizerFull2->Add(executeBoxSizer, 1, wxEXPAND | wxALL, 6);
    sizerFull2->Add(displayBoxSizer, 1, wxEXPAND | wxALL, 6);

    SetSizer(sizerFull2);


	wxBoxSizer *greedySizer = new wxBoxSizer(wxHORIZONTAL);
	mGreedyText = new wxTextCtrl(this, 1008, wxT("0"), wxDefaultPosition, wxSize(40,20), wxTE_RIGHT);
	greedySizer->Add(mGreedyText,2,wxALL,1);
	greedySizer->Add(new wxButton(this, button_UpdateGreedyRatio, wxT("Set G-Ratio")),2,wxALL,1);
	col1Sizer->Add(greedySizer,1,wxALL,2);
}

/**
 * @function OnRadio
 * @brief Handle Radio toggle
 */
void MultiPlannerTab::OnRadio(wxCommandEvent &evt) {

	mRrtStyle = evt.GetSelection();
	std::cout << "rrtStyle = " << mRrtStyle << std::endl;
}

/**
 * @function OnButton
 * @brief Handle Button Events
 */
void MultiPlannerTab::OnButton(wxCommandEvent &evt) {

  int button_num = evt.GetId();

  switch (button_num) {

    /** Set Start */
  case button_SetStart:
    if ( mWorld != NULL ) {
      if( mWorld->getNumRobots() < 1) {
	std::cout << "(!) Must have a world with a robot to set a Start state" << std::endl;
	break;
      }

//		if (robotId < 0) { // multi robot
//			int size = 0;
//			for (int i = 0; i < mWorld->getNumRobots(); i++) {
//				size += mWorld->getRobot(i)->getQuickDofs().size();
//			}
//			mStartConf.resize(size);
//
//			int offset = 0;
//			for (int i = 0; i < mWorld->getNumRobots(); i++) {
//				robotics::Robot* robot = mWorld->getRobot(i);
//				std::cout << "(i) Setting Start state for " << robot->getName() << ":" << std::endl;
//
//				//mStartConf = mWorld->getRobot(i)->getQuickDofs();
//				for (int j = 0; j < robot->getQuickDofs().size(); j++) {
//					mStartConf(offset+j) = robot->getQuickDofs()(j);
//				}
//				offset += robot->getQuickDofs().size();
//			}
//		} else {
//			mStartConf.resize(mWorld->getRobot(robotId)->getQuickDofs().size());
//			mStartConf = mWorld->getRobot(robotId)->getQuickDofs();
			// Single robot - multiple RRTs

			for (int i = 0; i < mWorld->getNumRobots(); i++) { // Loop accross the robots

                std::map<int,Eigen::VectorXd>::iterator it;
                it = mStartConfs.find(i);
                if (it == mStartConfs.end())
                    mStartConfs.insert(std::pair<int,Eigen::VectorXd>(i,mWorld->getRobot(i)->getQuickDofs()));
                else
                    it->second = mWorld->getRobot(i)->getQuickDofs();

			}




//		}



//      for( unsigned int i = 0; i < mStartConf.size(); i++ )
//	{  std::cout << mStartConf(i) << " ";  }
//      std::cout << std::endl;



    } // End of (If there is a world loaded)
    else {
      std::cout << "(!) Must have a world loaded to set a Start state." << std::endl;
    }
    break;

    /** Set Goal */
  case button_SetGoal:
    if ( mWorld != NULL ) {
      if( mWorld->getNumRobots() < 1){
	std::cout << "(!) Must have a world with a robot to set a Goal state.(!)" << std::endl;
	break;
      }

//		if (robotId < 0) { // multi robot
//			int size = 0;
//			for (int i = 0; i < mWorld->getNumRobots(); i++) {
//				size += mWorld->getRobot(i)->getQuickDofs().size();
//			}
//			mGoalConf.resize(size);
//
//			int offset = 0;
//			for (int i = 0; i < mWorld->getNumRobots(); i++) {
//				robotics::Robot* robot = mWorld->getRobot(i);
//				std::cout << "(i) Setting goal state for " << robot->getName() << ":" << std::endl;
//
//				//mGoalConf = mWorld->getRobot(i)->getQuickDofs();
//				for (int j = 0; j < robot->getQuickDofs().size(); j++) {
//					mGoalConf(offset+j) = robot->getQuickDofs()(j);
//				}
//				offset += robot->getQuickDofs().size();
//			}
//		} else {
//			//single robot
//			mGoalConf.resize(mWorld->getRobot(robotId)->getQuickDofs().size());
//			mGoalConf = mWorld->getRobot(robotId)->getQuickDofs();
//			// Single robot - multiple RRTs
//			std::map<int,Eigen::VectorXd>::iterator it;
//			it = mGoalConfs.find(robotId);
//			if (it == mGoalConfs.end())
//				mGoalConfs.insert(std::pair<int,Eigen::VectorXd>(robotId,mWorld->getRobot(robotId)->getQuickDofs()));
//			else
//				it->second = mWorld->getRobot(robotId)->getQuickDofs();
//		}
//
//      for( unsigned int i = 0; i < mGoalConf.size(); i++ )
//	{ std::cout << mGoalConf(i) << " "; }
//      std::cout << std::endl;


			for (int i = 0; i < mWorld->getNumRobots(); i++) { // Loop accross the robots

                std::map<int,Eigen::VectorXd>::iterator it;
                it = mGoalConfs.find(i);
                if (it == mGoalConfs.end())
                    mGoalConfs.insert(std::pair<int,Eigen::VectorXd>(i,mWorld->getRobot(i)->getQuickDofs()));
                else
                    it->second = mWorld->getRobot(i)->getQuickDofs();

			}


    } else {
      std::cout << "(!) Must have a world loaded to set a Goal state"<< std::endl;
    }
    break;

    /** Show Start */
  case button_showStart:
//    if( mStartConf.size() < 1 ){
//      std::cout << "(x) First, set a start configuration" << std::endl;
//      break;
//    }

//		if (robotId < 0) { // multi robot
//			int offset = 0;
//			for (int i = 0; i < mWorld->getNumRobots(); i++) {
//				robotics::Robot* robot = mWorld->getRobot(i);
//
//				Eigen::VectorXd start;
//				start.resize(robot->getQuickDofs().size());
//				for (int j = offset; j < offset+start.size(); j++) {
//					start(j-offset) = mStartConf(j);
//				}
//				offset += start.size();
//				robot->setQuickDofs(start);
//			}
//		} else {
//			// single robot
//			mWorld->getRobot(robotId)->setQuickDofs(mStartConf);
//		}
//	}
//
//    for( unsigned int i = 0; i< mStartConf.size(); i++ )
//      {
//		  std::cout << mStartConf(i) << " ";
//	}
//    std::cout << std::endl;

	for (int i = 0; i < mWorld->getNumRobots(); i++) {

        std::map<int,Eigen::VectorXd>::iterator it;
        it = mStartConfs.find(i);
        if (it == mStartConfs.end()){
            std::cout << "Robot " << mWorld->getRobot(i)->getName() << " doesn't have a Start position yet" << std::endl;
        }
        else{
            mWorld->getRobot(i)->setQuickDofs(mStartConfs[i]);
            mWorld->getRobot(i)->update();
        }
	}

    viewer->UpdateCamera();
    break;

    /** Show Goal */
  case button_showGoal:
//    if( mGoalConf.size() < 1 ){
//      std::cout << "(x) First, set a goal configuration" << std::endl;
//      break;
//    }
//	{
//		if (robotId < 0) { // multi robot
//			int offset = 0;
//			for (int i = 0; i < mWorld->getNumRobots(); i++) {
//				robotics::Robot* robot = mWorld->getRobot(i);
//
//				Eigen::VectorXd goal;
//				goal.resize(robot->getQuickDofs().size());
//				for (int j = offset; j < offset+goal.size(); j++) {
//					goal(j-offset) = mGoalConf(j);
//				}
//				offset += goal.size();
//				robot->setQuickDofs(goal);
//			}
//		} else {
//			// single
//			mWorld->getRobot(robotId)->setQuickDofs(mGoalConf);
//		}
//	}
//
//    for( unsigned int i = 0; i< mGoalConf.size(); i++ )
//      {  std::cout << mGoalConf[i] << " ";  }
//    std::cout << std::endl;
//
//	for (int i = 0; i < mWorld->getNumRobots(); i++) {
//		mWorld->getRobot(i)->update();
//	}



	for (int i = 0; i < mWorld->getNumRobots(); i++) {

        std::map<int,Eigen::VectorXd>::iterator it;
        it = mGoalConfs.find(i);
        if (it == mGoalConfs.end()){
            std::cout << "Robot " << mWorld->getRobot(i)->getName() << " doesn't have a Start position yet" << std::endl;
        }
        else{
            mWorld->getRobot(i)->setQuickDofs(mGoalConfs[i]);
            mWorld->getRobot(i)->update();
        }
	}


    viewer->UpdateCamera();
    break;

    /** Separate Plan Button - Execute plans individually and check collisions by merging plans */
  case button_planSeparate:{

       if( mWorld == NULL ){ std::cout << "(x) Must load a world" << std::endl; break; }
    if( mWorld->getNumRobots() < 1){ std::cout << "(x) Must load a world with a robot" << std::endl; break; }


    std::cout << "-- (0) Planning individually all robots (0)--" << std::endl;

      double stepSize = 0.02;
      bool result = true;
Eigen::VectorXi tempLinks;

	for (int i = 0; i < mWorld->getNumRobots(); i++) {

		if( mGoalConfs.find(i) == mGoalConfs.end() ){ std::cout << "(x) Must set a goal for Robot #" << i << std::endl; result = false;break; }
  		if( mStartConfs.find(i) == mStartConfs.end() ){ std::cout << "(x) Must set a start for Robot #" << i  << std::endl; result = false;break; }
      		if( mWorld == NULL ){ std::cout << "(x) Must load a world" << std::endl; result = false;break; }
      		if( mWorld->getNumRobots() < 1){ std::cout << "(x) Must load a world with a robot" << std::endl; result = false;break; }


		PathPlanner* planner0 = new PathPlanner(*mWorld,false,stepSize);
		tempLinks = mWorld->getRobot(i)->getQuickDofsIndices();
		int maxNodes = 5000;


		std::cout << "-- (0) Planning individually Robot " << mWorld->getRobot(i)->getName() << std::endl;

    		result = result && planner0->planPath(
					i,
					tempLinks,
					mStartConfs[i],
					mGoalConfs[i],
					std::vector<int>(),
					mRrtStyle,
					mConnectMode,
					mGreedyMode,
					mGreedyRatio,
					mSmooth,
					maxNodes);


		// Creates or Updates the path
			std::map<int,std::list<Eigen::VectorXd> >::iterator it;
			it = paths.find(i);
			if (it == paths.end()){
				paths.insert(std::pair<int,std::list<Eigen::VectorXd> >(i,planner0->path));
				std::cout << "Creating path entry for Robot " << mWorld->getRobot(i)->getName()	<< std::endl;
			}
			else{
				it->second = planner0->path;
				std::cout << "Updating path entry for Robot " << mWorld->getRobot(i)->getName()	<< std::endl;
			}

			delete planner0;

	}

	if (result){



		// Concatenates plans
		std::cout << "Mergin plans in High-dimension space" << std::endl;
		// New empty plan
		std::list<Eigen::VectorXd> HDPlan;
		// Gets the size of the space
		int size = 0;
		bool done = false;
		for (int i = 0; i < mWorld->getNumRobots(); i++) {
				size += mWorld->getRobot(i)->getQuickDofs().size();
		}
		int path_pos = 0;

		while (!done){

			done = true;
			int offset = 0;
			Eigen::VectorXd pose;
			pose.resize(size);
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				std::list<Eigen::VectorXd>::iterator it = paths[i].begin();

				bool armDone = (path_pos>=paths[i].size());
				if (armDone){
					it = paths[i].end();
					it--;
				}// Repeats last position for arms with shorter paths
				else{ // Arrm not done yet, so we get to the position in the plan
					for (int m = 0; m<path_pos; m++)
					it++;
				}
				done = done && armDone;
				int armSize = mWorld->getRobot(i)->getQuickDofs().size();

				for (int j=0; j< armSize;j++){ // This loop fills the HD plan, link by link for arm i
					pose(offset+j) = (*it)[j];
				}
				//Adds the pose to the plan
				HDPlan.push_back(pose);

				offset += armSize;



			}// End for i (all arms considered)

		path_pos++;


		}// End while (Plan concatenation done)

	//mPlanner->path = HDLine;

	SetTimeline(HDPlan);

	}// End if result (All plans worked)
	else{

		std::cout << "At least one of the plans could not be calculated - Process canceled" << std::endl;
	}



}
    break;

    /** Execute Plan */
  case button_planHD:
    {

    if( mWorld == NULL ){ std::cout << "(x) Must load a world" << std::endl; break; }
    if( mWorld->getNumRobots() < 1){ std::cout << "(x) Must load a world with a robot" << std::endl; break; }

        // Computing HD Start and Goal State
        int size = 0;
        for (int i = 0; i < mWorld->getNumRobots(); i++) {
            size += mWorld->getRobot(i)->getQuickDofs().size();
        }
        mGoalConf.resize(size);
        mStartConf.resize(size);
        mLinks.resize(size);

        int offset = 0;
        for (int i = 0; i < mWorld->getNumRobots(); i++) {
            robotics::Robot* robot = mWorld->getRobot(i);
            std::cout << "(i) Setting start and goal state for " << robot->getName() << ":" << std::endl;
             std::map<int,Eigen::VectorXd>::iterator itGoal,itStart;
             itGoal = mGoalConfs.find(i);
             itStart = mStartConfs.find(i);
            if (itGoal == mGoalConfs.end() || itGoal == mGoalConfs.end()){
                std::cout << "Robot " << mWorld->getRobot(i)->getName() << " doesn't have a Start position or a Goal position yet" << std::endl;
                break;
            }
            else {
            //mGoalConf = mWorld->getRobot(i)->getQuickDofs();
                for (int j = 0; j < robot->getQuickDofs().size(); j++) {
                    mGoalConf(offset+j) = itGoal->second(j);
                    mStartConf(offset+j) = itStart->second(j);
                    mLinks(j+offset) = robot->getQuickDofsIndices()(j);

                    //std::cout << mLinks << std::endl << std::endl;

                }
                offset += robot->getQuickDofs().size();
            }
        }

      double stepSize = 0.02;

      mPlanner = new PathPlanner( *mWorld, false, stepSize );

      int maxNodes = 10000;
      bool result = mPlanner->planPath(
					robotId,
					mLinks,
					mStartConf,
					mGoalConf,
					std::vector<int>(),
					mRrtStyle,
					mConnectMode,
					mGreedyMode,
					mGreedyRatio,
					mSmooth,
					maxNodes);
      if( result  )
	{  SetTimeline(); }
    }
    break;

    /** Update Time */
  case button_UpdateTime:
    {
      /// Update the time span of the movie timeline
      SetTimeline();
    }
    break;

  case button_UpdateGreedyRatio:
	  {
		  long T;
		  mGreedyText->GetValue().ToLong(&T);
		  if( T > 10 || T < 1 )
		  {
			  std::cout << "(x) Must be value between 1-10.  Setting to 0 (default). Greedy Ratio = 0%" << std::endl;
			  mGreedyRatio = (int) 0;
			  return;
		  } else {
			  std::cout << "(!) Greedy Ratio = " << T << "0%" << endl;
			  mGreedyRatio = (int) T;
		  }
	  }
	  break;

    /** Show Path */
  case button_ShowPath:
    if( mWorld == NULL || mPlanner == NULL || mPlanner->path.size() == 0 ) {
      std::cout << "(!) Must create a valid plan before printing."<<std::endl;
      return;
    } else {
      std::cout<<"(i) Printing...Implement me :)"<<std::endl;
    }
    break;
  }
}


void MultiPlannerTab::SetTimeline(std::list<Eigen::VectorXd> path) {

	double T;
    mTimeText->GetValue().ToDouble(&T);

    int numsteps = path.size();
    double increment = T/(double)numsteps;

    cout << "-->(+) Updating Timeline - Increment: " << increment << " Total T: " << T << " Steps: " << numsteps << endl;

    frame->InitTimer( string("RRT_Plan"),increment );

    //Eigen::VectorXd vals( mLinks.size() );

    for( std::list<Eigen::VectorXd>::iterator it = path.begin(); it != path.end(); it++ ) {

		//std::cout << "Rendering frame " << turn << " (" << mPlanner->path.size() << ")\n";


			int offset = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				robotics::Robot* robot = mWorld->getRobot(i);

				Eigen::VectorXd pose;
				pose.resize(robot->getQuickDofs().size());
				for (int j = offset; j < offset+pose.size(); j++) {
					pose(j-offset) = (*it)(j);
				}
				offset += pose.size();
				robot->setQuickDofs(pose);
				robot->update();
			}


        frame->AddWorld( mWorld );
    }



}

/**
 * @function setTimeLine
 * @brief
 */
void MultiPlannerTab::SetTimeline() {

    if( mWorld == NULL || mPlanner == NULL || mPlanner->path.size() == 0 ) {
        cout << "--(!) Must create a valid plan before updating its duration (!)--" << endl;
	return;
    }

    double T;
    mTimeText->GetValue().ToDouble(&T);

    int numsteps = mPlanner->path.size();
    double increment = T/(double)numsteps;

    cout << "-->(+) Updating Timeline - Increment: " << increment << " Total T: " << T << " Steps: " << numsteps << endl;

    frame->InitTimer( string("RRT_Plan"),increment );

    Eigen::VectorXd vals( mLinks.size() );

    for( std::list<Eigen::VectorXd>::iterator it = mPlanner->path.begin(); it != mPlanner->path.end(); it++ ) {

		//std::cout << "Rendering frame " << turn << " (" << mPlanner->path.size() << ")\n";

		if (robotId < 0) { // multi
			int offset = 0;
			for (int i = 0; i < mWorld->getNumRobots(); i++) {
				robotics::Robot* robot = mWorld->getRobot(i);

				Eigen::VectorXd pose;
				pose.resize(robot->getQuickDofs().size());
				for (int j = offset; j < offset+pose.size(); j++) {
					pose(j-offset) = (*it)(j);
				}
				offset += pose.size();
				robot->setQuickDofs(pose);
				robot->update();
			}
		}

		else { // single
			mWorld->getRobot(robotId)->setQuickDofs(*it);
			mWorld->getRobot(robotId)->update();
		}

        frame->AddWorld( mWorld );
    }

}

/**
 * @function OnCheckBox
 * @brief Handle CheckBox Events
 */
void MultiPlannerTab::OnCheckBox( wxCommandEvent &evt ) {
  int checkbox_num = evt.GetId();

  switch (checkbox_num) {

  case checkbox_beGreedy:
    mGreedyMode = (bool)evt.GetSelection();
    std::cout << "(i) greedy = " << mGreedyMode << std::endl;
    break;

  case checkbox_useConnect:
    mConnectMode = (bool)evt.GetSelection();
    std::cout << "(i) useConnect = " << mConnectMode << std::endl;
    break;
  case checkbox_useSmooth:
    mSmooth = (bool)evt.GetSelection();
    std::cout << "(i) Smooth option = " << mSmooth << std::endl;
    break;
  }
}

/**
 * @function OnSlider
 * @brief Handle slider changes
 */
void MultiPlannerTab::OnSlider(wxCommandEvent &evt) {
  if (selectedTreeNode == NULL) {
    return;
  }

  int slnum = evt.GetId();
  double pos = *(double*) evt.GetClientData();
  char numBuf[1000];

  switch (slnum) {
  case slider_Time:
    sprintf(numBuf, "X Change: %7.4f", pos);
    std::cout << "(i) Timeline slider output: " << numBuf << std::endl;
    //handleTimeSlider(); // uses slider position to query plan state
    break;

  default:
    return;
  }
  //world->updateCollision(o);
  //viewer->UpdateCamera();

  if (frame != NULL)
    frame->SetStatusText(wxString(numBuf, wxConvUTF8));
}

/**
 * @function GRIPStateChange
 * @brief This function is called when an object is selected in the Tree View or other
 *        global changes to the RST world. Use this to capture events from outside the tab.
 */
void MultiPlannerTab::GRIPStateChange() {
	//init_objs();


	if ( selectedTreeNode == NULL ) {
		return;
	}

	std::string statusBuf;
	std::string buf, buf2;

	switch (selectedTreeNode->dType) {

	case Return_Type_Object:
		mSelectedObject = (robotics::Object*) ( selectedTreeNode->data );
		statusBuf = " Selected Object: " + mSelectedObject->getName();
		buf = "You clicked on object: " + mSelectedObject->getName();

		// Enter action for object select events here:

		break;
	case Return_Type_Robot:
		mSelectedRobot = (robotics::Robot*) ( selectedTreeNode->data );
		statusBuf = " Selected Robot: " + mSelectedRobot->getName();
		buf = " You clicked on robot: " + mSelectedRobot->getName();

		// Enter action for Robot select events here:

		break;
	case Return_Type_Node:
		mSelectedNode = (dynamics::BodyNodeDynamics*) ( selectedTreeNode->data );
		statusBuf = " Selected Body Node: " + string(mSelectedNode->getName()) + " of Robot: "
			+ ( (robotics::Robot*) mSelectedNode->getSkel() )->getName();
		buf = " Node: " + std::string(mSelectedNode->getName()) + " of Robot: " + ( (robotics::Robot*) mSelectedNode->getSkel() )->getName();

		// Enter action for link select events here:

		break;
	default:
		fprintf(stderr, "--( :D ) Someone else's problem!\n");
		assert(0);
		exit(1);
	}

	//cout << buf << endl;
	frame->SetStatusText(wxString(statusBuf.c_str(), wxConvUTF8));
	sizerFull2->Layout();
}
